#include "PointLabTools/Filter/RandomSample.h"

namespace PointlabTools
{

	RandomSample::RandomSample():
		mInputCloud(nullptr),
		mPercentage(100)
	{

	}

	RandomSample::~RandomSample()
	{

	}

	void RandomSample::setInputCloud(PointLabCore::IPointCloud* cloud)
	{
		mInputCloud = cloud;
	}

	PointLabCore::IPointCloud* RandomSample::getInputCloud()
	{
		return mInputCloud;
	}

	PointLabCore::IPointCloud* RandomSample::execute()
	{
		PointLabCore::IPointCloud* newCloud = PointLabCore::ClientClass::createPointCloudInstance();

		PointLabCore::Uint ptsNum = mInputCloud->getPointNum();

		std::vector<PointLabCore::Uint> indexs;
		indexs.resize(ptsNum);

		for (PointLabCore::Uint i = 0; i < ptsNum; i++)
		{
			indexs[i] = i;
		}

		PointLabCore::Uint newNumberOfPoints = static_cast<PointLabCore::Uint>(ptsNum*mPercentage / 100.0);
		PointLabCore::Uint pointsToRemove = ptsNum - newNumberOfPoints;
		PointLabCore::Uint lastPointindex = ptsNum - 1;

		std::random_device rd;
		std::mt19937 gen(rd());

		for (PointLabCore::Uint i = 0; i < pointsToRemove; i++)
		{
			std::uniform_int_distribution<PointLabCore::Uint> dist(0, lastPointindex);
			PointLabCore::Uint index = dist(gen);
			std::swap(indexs[index], indexs[lastPointindex]);
			--lastPointindex;
		}

		indexs.resize(newNumberOfPoints);

		for (PointLabCore::Uint i = 0; i < indexs.size(); i++)
		{
			newCloud->appendPointCoord(mInputCloud->getPointCoord(indexs[i]));

			if (mInputCloud->hasNormal())
			{
				newCloud->appendPointNormal(mInputCloud->getPointNormal(indexs[i]));
			}

			if (mInputCloud->hasColor())
			{
				newCloud->appendPointColor(mInputCloud->getPointColor(indexs[i]));
			}
		}

		newCloud->setName(mInputCloud->getName() + std::string(".sub"));
		return newCloud;
	}

}